function [ vN_m, DvSFL_n1_m, DvGCorN_m, CL_n1L_m ] = vel_ns( vN_m1, vN_m2, sumDrN_m1, DvSFB_m1_m, CB_m1L_n1, CL_n1L_m1, ...
    earthPar_n1, earthPar_n2, j, r, Tm, cst ) %#codegen
%VEL_NS 中速速度解算
%
% Input Arguments:
% j: 低速循环中的中速循环个数
% r: 上次低速循环开始以来的中速循环计数
% fcnDvrot_m: 计算Dvrot_m的函数句柄
%
% References:
% [1] 理论文档“捷联惯性导航数值积分算法”， % 青鸟版本1.3 %

%%
DvSFL_n1_m = CB_m1L_n1 * DvSFB_m1_m; % REF1式8.45后半部分

[ omegaIEN_n1_m, rhoNz_n1_m, FCN_n1_m ] = extrapgeometricearthpar(earthPar_n1, earthPar_n2, j, r/2); % REF1式8.48
DrN_n1_m = sumDrN_m1 + (3*vN_m1-vN_m2)*Tm/2; % REF1式8.49
zeta_n1_m = cst.CNL * (omegaIEN_n1_m*r*Tm + rhoNz_n1_m*cst.uZNN*r*Tm + FCN_n1_m*cross(cst.uZNN, DrN_n1_m));% REF1式8.47
CL_n1L_m = eye(3) - skewsymmat(zeta_n1_m); % REF1式8.46，(一阶形式)
DvSFL_m = (CL_n1L_m + CL_n1L_m1) * DvSFL_n1_m / 2; % REF1式8.45前半部分

gN_mh = extrapolate(earthPar_n1.gN, earthPar_n2.gN, j, r-1/2); % REF1式8.41
[ omegaIEN_mh, rhoNz_mh, FCN_mh ] = extrapgeometricearthpar(earthPar_n1, earthPar_n2, j, r-0.5);
vN_mh = (3*vN_m1 - vN_m2) / 2; % REF1式8.40
DvGCorN_m = (gN_mh - cross(2*omegaIEN_mh + rhoNz_mh*cst.uZNN + FCN_mh*cross(cst.uZNN, vN_mh), vN_mh)) * Tm; % REF1式8.39

vN_m = vN_m1 + cst.CNL'*DvSFL_m + DvGCorN_m; % REF1式8.36
end

%%
function [ x_r ] = extrapolate( x_n1, x_n2, j, r )
x_r = x_n1 + (x_n1 - x_n2) * (r/j);
end

function [ omegaIEN, rhoNz, FCN ] = extrapgeometricearthpar(earthPar_n1, earthPar_n2, j, r)
omegaIEN = extrapolate(earthPar_n1.omegaIEN, earthPar_n2.omegaIEN, j, r);
rhoNz = extrapolate(earthPar_n1.rhoNz, earthPar_n2.rhoNz, j, r);
FCN = extrapolate(earthPar_n1.FCN, earthPar_n2.FCN, j, r);
end